#ifndef __QMI8658A_H__
#define __QMI8658A_H__


#include <driver/i2c_master.h>
#include <cstdint>

class QMI8658A
{
 
#define ACCRANGE                16      //加速度计量程
#define GYRRANGE                2048    //陀螺仪量程
#define SAMPLERATE              800.0f  //采样频率 
#define MIN_COLLECTION_COUNT    1000    // 最小采集数据次数
#define USED_DATA_COUNT         50      // 用于计算平均值的数据数量
#define STATIONARY_THRESHOLD    0.001f  // 判断静止的加速度模长差值阈值

#define CTRL1                   0x02  //配置通讯方式、中断引脚、fifo中断，暂时配置为6C
#define CTRL2           0x03  //配置加速度计
#define CTRL3           0x04  //配置陀螺仪
#define CTRL5           0x06  //设置滤波器
#define CTRL7           0x08  //启用加速度计和陀螺仪
#define CTRL8           0x09  //运动检测
#define CTRL9           0x0A  //CTRL9执行预定指令

#define FIFO_WTM_TH     0x13  //FIFO 水位标记，设置触发值
#define FIFO_CTRL       0x14  //FIFO控制寄存器
#define FIFO_SMPL_CNT   0x15  //FIFO样本计数寄存器
#define FIFO_STATUS     0x16  //FIFO状态寄存器
#define FIFO_DATA       0x17  //FIFO输出寄存器

#define STATUSINT       0x2D  //传感器数据可用和锁存寄存器
#define STATUS1         0x2F  //杂项状态寄存器（运动、步数、点击）
#define TIMESTAMP       0x30  //时间戳（0x30-0x32）

#define TEMP_L          0x33  //温度 = TEMP_H + (TEMP_L / 256)
#define TEMP_H          0x34  

#define A_XYZ           0x35  //加速度输出寄存器(0x35–0x3A)
#define G_XYZ           0x3B  //陀螺仪输出寄存器(0x3B–0x40)

#define COD_STATUS      0x46  //按需校准状态寄存器

#define TAP_STATUS      0x59  //敲击状态寄存器
#define STEP_COUNT      0x5A  //步数计数寄存器（0x5A-0x5C）

#define RESET           0x60  //软件复位寄存器，任何模式写入0xB0复位
#define dQY_L           0x4D  //如果有成功的复位（上电复位或软复位）过程，寄存器 0x4D 的值将等于 0x80

// 主机控制校准寄存器（见 CTRL9，可选择使用）
#define CAL1_L          0x0B
#define CAL1_H          0x0C
#define CAL2_L          0x0D
#define CAL2_H          0x0E
#define CAL3_L          0x0F
#define CAL3_H          0x10
#define CAL4_L          0x11
#define CAL4_H          0x12

public:

    float gyro_compensate[6];   // 陀螺仪补偿值

private:
    /* data */
    i2c_master_dev_handle_t m_i2c_dev;
    uint8_t m_i2c_addr;
public:
    QMI8658A(/* args */) {}
    ~QMI8658A() {}

    /// @brief 初始化QMI8658A传感器
    /// @param i2c_bus  I2C总线句柄
    /// @param addr I2C地址，默认值为0x6A
    void init(i2c_master_bus_handle_t i2c_bus, uint8_t addr = 0x6A);

    /// @brief 
    void deinit();

    /// @brief 加速度计自检
    /// @return 0表示成功，-1表示失败
    int self_test_acc();

    /// @brief 陀螺仪自检
    /// @return 0表示成功，-1表示失败
    int self_test_gyro();

    /// @brief 读取传感器原始数据
    /// @param raw_data 存储读取到的原始数据，长度至少为6
    /// @return -1读取失败，0读取成功
    int read_raw_data(int16_t *raw_data);

    /// @brief 读取传感器数据，转换并打印
    int read_convert_print_data();

    /// @brief 对陀螺仪进行校准，并将校准结果存储到传入的数组中
    /// @param outdata 长度至少为3的数组，用于存储校准后的陀螺仪数据
    /// @return 0 表示成功，-1表示失败
    int calibration_acc_gry(float *outdata);

    /// @brief 对陀螺仪进行按需校准(COD, Calibration On Demand)操作
    /// @return 0表示成功，-1表示失败
    int cyr_cod(void);

    /// @brief 根据加速度计和陀螺仪的量程，将原始数据转换为实际的物理值
    /// @param in_data 原始数据，数组长度为6
    /// @param outdata 转换后的数据，数组长度为6
    /// @param accele_range 加速度计的量程
    /// @param gyro_range 陀螺仪的量程
    void convert_data(int16_t *in_data, float *outdata, int accele_range, int gyro_range);

    /// @brief 计算加速度计的模长
    /// @param outdata 数组长度为3，数据为加速度计的x、y、z轴数据
    /// @return 加速度的模长
    float calculate_accele_magnitude(float *outdata);

    /// @brief 计算陀螺仪平均值的辅助函数
    /// @param data 陀螺仪多个采集点的数据
    /// @param outdata 长度至少为3的数组，用于存储计算后的平均值
    void calculate_gyro_averages(float data[MIN_COLLECTION_COUNT][3], float *outdata);

    /// @brief 获取单位g的三周加速度计和单位为dps的三轴陀螺仪数据
    /// @param outdata 存储转换后的数据，长度至少为6
    void get_g_dps(float *outdata);

    static int compare_float(const void *a, const void *b)
    {
        return (*(float *)a - *(float *)b) > 0 ? 1 : -1;
    }

private:
    /// @brief 写入寄存器
    /// @param cmd 寄存器地址
    /// @param value 写入的值
    /// @param count 写入的字节数
    /// @return 0表示成功，-1表示失败
    int write_regs(uint8_t cmd, uint8_t *value, uint8_t count);

    /// @brief 读取寄存器
    /// @param cmd 寄存器地址
    /// @param value 存储读取到的值
    /// @param count 读取的字节数
    /// @return 0表示成功，-1表示失败
    int read_regs(uint8_t cmd, uint8_t *value, uint8_t count);

    /// @brief  计算陀螺仪平均值
    /// @param data 
    /// @param outdata 
    void calculate_averages(float data[MIN_COLLECTION_COUNT][6], float *outdata);

};




#endif // __QMI8658A_H__

